/*
 * ekf.h
 *
 * Created: 9/17/2011 2:17:38 PM
 *  Author: GrubyGrub
 */ 


#ifndef EKF_H_
#define EKF_H_
#include "marg.h"

#define DEG2RAD 0.0174532925
#define RAD2DEG 57.2957796


#define GYRO_500DPS 17.50

#define GYRO_DEGLSB (GYRO_500DPS/1024.00)

#define MAG_COV 0.3
#define ACC_COV 0.6
#define P_COV 0.4
#define Q_COV 0.0000001

//Acc Reference
#define ACC_RX -0.0213
#define ACC_RY  0.0116
#define ACC_RZ  0.9997

//Mag reference
#define MAG_RX  0.0054
#define MAG_RY -0.2910
#define MAG_RZ -0.9566


//Mag bias
#define MAG_BX -108.4814
#define MAG_BY    9.5291
#define MAG_BZ   21.3575

//Gyro Bias
#define GYRO_BX -14.5838
#define GYRO_BY 47.9794
#define GYRO_BZ -43.3941

//Mag Calibration
#define MAG_CXX  0.2013789977353
#define MAG_CXY  0.0023528392492
#define MAG_CXZ -0.0049776402801
#define MAG_CYX  0
#define MAG_CYY  0.1996831018998
#define MAG_CYZ -0.0009618099551
#define MAG_CZX  0
#define MAG_CZY  0
#define MAG_CZZ  0.1953393843802












/*acc reference vectors*/
static float acc_ref_x;
static float acc_ref_y;
static float acc_ref_z;
static float acc_ref[3];
static float acc_frame[3];

/*mag reference vectors*/
static float mag_ref_x;
static float mag_ref_y;
static float mag_ref_z;
static float mag_ref[3];
static float mag_frame[3];


/*gyro bias*/
static float gyro_bias_x;
static float gyro_bias_y;
static float gyro_bias_z;

/*mag bias*/
static float mag_bias_x;
static float mag_bias_y;
static float mag_bias_z;

/* mag calibration matrix (may not be exactly the same
MagCMatrix = [xx xy xz;
              yx yy yz;
			  zx zy zz]
			  */
static float mag_cali_xx;
static float mag_cali_xy;
static float mag_cali_xz;

static float mag_cali_yx;
static float mag_cali_yy;
static float mag_cali_yz;

static float mag_cali_zx;
static float mag_cali_zy;
static float mag_cali_zz;

/*predicted estimate covariance*/
static float P[7][7];

/*Innovation covariance*/
static float Q[7][7];

/*State Jacobian matrix*/
static float Fj[7][7];

/*swaps*/
static float swap1[7][7];
static float swap2[7][7];

/*acclermeter covariance*/
static float Racc[3][3];

/*magnetomer covariance*/
static float Rmag[3][3];

/*the kalman state vector*/
static float state_q0;
static float state_q1;
static float state_q2;
static float state_q3;
static float state_gb0;
static float state_gb1;
static float state_gb2;

/*magnetomer reference rotation matrix*/
static float MagRefM[3][3];
static float MagFrameM[3][3];

/*state vector*/
static float state[7];



extern void ekf_init(struct marg_sensors_t * marg);
extern void process_gyro(float gx, float gy, float gz, float dT);
extern void process_mag(float mx, float my, float mz);
extern void process_acc(float ax, float ay, float az);
















#endif /* EKF_H_ */